/***************************************************************************************
 The copyright in this software is being made available under the BSD License, included
 below. This software may be subject to other third party rights,including patent rights,
 and no such rights are granted under this license.

 Regents of the Tsinghua University, Peking University, Zhejiang University, HKUST
 Tsinghua University, Peking University, Zhejiang University, HKUST 2011

 Copyright (c) 2011, Regents of the Tsinghua University, Peking University, Zhejiang
 University, HKUST. All Rights Reserved.

 Redistribution and use in source and binary forms, with or without modification, are
 permitted provided that the following conditions are met:
   * Redistributions of source code must retain the above copyright notice,
     this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright notice,
     this list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.
   * Neither the names of the copyright holders nor the names of its
     contributors may be used to endorse or promote products derived from this
     software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 OF SUCH DAMAGE.
***************************************************************************************/


#include <stdlib.h>
#include <math.h>
#include <memory.h>
#include <assert.h>
#include <time.h>
#include <sys/timeb.h>

#include "contributors.h"
#include "memalloc.h"
#include "block.h"
#include "vlc.h"
#include "macroblock.h"
#include "ratectl.h"
#include "global.h"
#ifdef FastME
#include "fast_me.h"
#include "qcoder.h"
#endif

#include "adaptiverounding.h"  //N11 liwr

static int best_mode; //, best_weight_flag, best_ref_frame[2][2], b8pdir[2][2];
//static byte mb_y_rec[MB_BLOCK_SIZE][MB_BLOCK_SIZE], mb_u_rec[B8_SIZE][B8_SIZE], mb_v_rec[B8_SIZE][B8_SIZE];
int cbp;
int best8x8ref[MAX_MODE_NUM][2][2]; // for B frame, it is used as the prediction direction
int best4x4ref[4][4];
static cs_ptr_t cs_mb = NULL;
//cs_ptr_t cs_abt=NULL;
//cs_ptr_t cs_abt8in4=NULL;
cs_ptr_t cs_sb=NULL;
cs_ptr_t cs_sb_best=NULL;
static int valid[MAX_MODE_NUM];
best_para_t best_parameter;

int   ****cofAC=NULL, ****cofAC8x8=NULL;        // [8x8block][4x4block][level/run][scan_pos]
int   ***cofDC=NULL;                       // [yuv][level/run][scan_pos]
int   ***cofAC4x4=NULL, ****cofAC4x4intern=NULL; // [level/run][scan_pos]

static int get_best_ref_and_cost( int *p_cost, int mode, int block );
/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
cs_ptr_t create_coding_state()
{
    cs_ptr_t cs;
    if ( ( cs = ( cs_ptr_t ) calloc( 1, sizeof( cs_obj_t ) ) ) == NULL )
        no_mem_exit( "init_coding_state: cs" );
    if ( ( cs->bitstream = ( bitstream_t * ) calloc( 1, sizeof( bitstream_t ) ) ) == NULL )
        no_mem_exit( "init_coding_state: cs->bitstream" );

    if( ( cs->qcoder = ( qcoder_env_t * )calloc( 1, sizeof( qcoder_env_t ) ) ) == NULL )
        no_mem_exit( "init_coding_state: cs->qcoder" );

    return cs;
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
void delete_coding_state( cs_ptr_t cs )
{
    if ( cs != NULL )
    {
        if ( cs->bitstream != NULL )
            free( cs->bitstream );

        if( cs->qcoder != NULL )
            free( cs->qcoder );

        free( cs );
        cs = NULL;
    }
}
/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
void clear_rdopt()
{
    free_mem_DCcoeff( cofDC );
    free_mem_ACcoeff( cofAC );
    free_mem_ACcoeff( cofAC8x8 );
    free_mem_ACcoeff( cofAC4x4intern );
    delete_coding_state( cs_mb );
    delete_coding_state( cs_sb );
    delete_coding_state( cs_sb_best );
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
void init_rdopt()
{
    get_mem_DCcoeff( &cofDC );
    get_mem_ACcoeff( &cofAC );
    get_mem_ACcoeff( &cofAC8x8 );
    get_mem_ACcoeff( &cofAC4x4intern );
    cofAC4x4 = cofAC4x4intern[0];
    cs_mb = create_coding_state();
    cs_sb = create_coding_state();
    cs_sb_best= create_coding_state();

}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static void set_mode_and_ref( int mode, macroblock_t *currMB )
{
    int i, j, k, m, ref_idx;

    currMB->mb_type = mode;
    memcpy( currMB->b8_ref, best8x8ref[mode],  4*sizeof( int ) );
    memcpy( currMB->b4_ref, best4x4ref,  16*sizeof( int ) );
    if( mode == INTER16x16 || mode == SKIP )
        memset( currMB->subblock_type,    0, 4*sizeof( int ) );
    switch ( mode )
    {
        case SKIP:
            break;
        case INTER16x16:
        case INTER8x8:
            memset( currMB->mvd,    0, 2*2*sizeof( mvd_t ) );
            memset( currMB->bi_mvd, 0, 2*2*sizeof( bi_mvd_t ) );
            for ( j = 0; j< 2; j++ )
            {
                for ( i = 0; i < 2; i++ )
                {
                    ref_idx = best8x8ref[mode][j][i];
                    if ( ref_idx != BI ) //single direction
                    {
                        currMB->mvd[j][i] = img->mvd[ref_idx][mode][j][i];
                    }
                    else
                    {
                        currMB->bi_mvd[j][i] = img->bi_mvd[mode][j][i];
                    }
                }
            }

            for ( j=0; j<2; j++ )
            {
                for ( i=0; i<2; i++ )
                {
                    currMB->b8mode[j][i] = INTRA_NO;
                }
            }
            currMB->uvmode = INTRA_NO;
            currMB->b16mode = INTRA_NO;
            for ( i=0; i<2; i++ )
                for ( j=0; j<2; j++ )
                    for ( k=0; k<2; k++ )
                        for ( m=0; m<2; m++ )
                            currMB->b4mode[i][j][k][m] = INTRA_NO;
            break;
        case INTER4x4:
            memset( currMB->mvd_4x4,    0, 4*4*sizeof( mvd_t ) );
            memset( currMB->bi_mvd_4x4, 0, 4*4*sizeof( bi_mvd_t ) );
            for ( j = 0; j< 4; j++ )
            {
                for ( i = 0; i < 4; i++ )
                {
                    ref_idx = best4x4ref[j][i];
                    if ( ref_idx != BI ) //single direction
                    {
                        currMB->mvd_4x4[j][i] = img->mvd_4x4[ref_idx][j][i];
                    }
                    else
                    {
                        currMB->bi_mvd_4x4[j][i] = img->bi_mvd_4x4[j][i];
                    }
                }
            }
            break;
        case I8MB:
            for ( j=0; j<2; j++ )
            {
                for ( i=0; i<2; i++ )
                {
                    currMB->b8mode[j][i] = INTRA_NO;
                }
            }
            currMB->uvmode = INTRA_NO;
            currMB->b16mode = INTRA_NO;
            for ( i=0; i<2; i++ )
                for ( j=0; j<2; j++ )
                    for ( k=0; k<2; k++ )
                        for ( m=0; m<2; m++ )
                            currMB->b4mode[i][j][k][m] = INTRA_NO;
            break;
        default:
            printf( "Unsupported mode in set_mode_and_ref!\n" );
            exit( 1 );
    }
}


/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static int get_recon_distortion()
{
    int y, x, j, i, uv, d = 0;
    for ( y = 0; y < MB_BLOCK_SIZE; y++ )
    {
        j = img->pix_y + y;
        for ( x = 0; x < MB_BLOCK_SIZE; x++ )
        {
            i  = img->pix_x + x;
            d += img->quad[img_y_org[j][i] - p_img_y_rec[j][i]];
        }
    }
    for ( uv = 0; uv < UV; uv++ )
    {
        for ( y = 0; y < B8_SIZE; y++ )
        {
            j = img->pix_c_y + y;
            for ( x = 0; x < B8_SIZE; x++ )
            {
                i = img->pix_c_x + x;
                d += img->quad[img_uv_org[uv][j][i] - p_img_uv_rec[uv][j][i]];
            }
        }
    }
    return d;
}


/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static double get_intra_rdcost()
{
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];
    int d, r;
    int j,i;
    double rdcost;
    double min_rdcost    = MAX_VALUE;
    int tmp_transform_16x16;
    byte tmp16_p_img_y_rec[16][16];

    if ( input->ABTen && input->IABTen )
    {
        //note: add 4x4 Trans RD calculation process
#if RDO_STAT
        int bits[5];
        tmp_transform_16x16 = 0;
        bits[0] = write_skip_flag();
        bits[1] = qcoder_encode_blocksize( qcoder, 1 );
        bits[2] = qcoder_encode_pred_direction( qcoder, INTRA );
        bits[3] = intra_luma_residual_coding( TRANS8 );
        bits[4] = intra_chroma_residual_coding();
        r = bits[0]+bits[1]+bits[2]+bits[3]+bits[4];
        d  = get_recon_distortion();
        rdcost = d + img->lambda_mode * r;
        {
            int x,y;
            printf( "MB:%3d,  Mode: INTRA, Rate: %6d, Distortion: %6d, Rdcost: %6.2f, (Bits: Skip Flag: %3d, Blocksize: %3d, PredDirect: %3d, ResY: %3d, ResUV: %3d)\n\n", img->curr_mb_nr, r, d, rdcost, bits[0], bits[1], bits[2], bits[3], bits[4] );
        }
#else
        tmp_transform_16x16 = 0;
        r  = write_skip_flag();
        r += qcoder_encode_blocksize( qcoder, 1 );
        r += qcoder_encode_pred_direction( qcoder, INTRA );
        r += intra_luma_residual_coding( TRANS8 );
        r += intra_chroma_residual_coding();
        d  = get_recon_distortion();
        rdcost = d + img->lambda_mode * r;
#endif
        currMB->transform_16x16 = tmp_transform_16x16;
        if ( rdcost<min_rdcost )  //  8x8 and 4x4 rdcost < 16x16 rdcost
        {
            currMB->transform_16x16 = 0;
            min_rdcost = rdcost;
        }
        else    //  choose 16x16
        {
            //  reset reconstructed pic
            for( j=0; j<16; j++ )
            {
                for( i=0; i<16; i++ )
                {
                    // This function is not working. Exit if it is accidentally called.
                    error( "This code is known not to work", 400 );
                    p_img_y_rec[img->pix_y+j][img->pix_x+i] = tmp16_p_img_y_rec[j][i];
                }
            }
            for ( i=0; i<2; i++ )
                for ( j=0; j<2; j++ )
                    currMB->y_dc[i][j] = currMB->y_16x16_dc;
        }
        rdcost = min_rdcost;
    }
    else
    {
        r = write_skip_flag();
        r += qcoder_encode_blocksize( qcoder, 1 );
        r += qcoder_encode_pred_direction( qcoder, INTRA );
        r += intra_luma_residual_coding( NON_ABT );
        r += intra_chroma_residual_coding();
        d  = get_recon_distortion();
        rdcost = d + img->lambda_mode * r;
    }

    return rdcost;
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
#if RDO_STAT
static double y_coding_block( int block8x8,int submode,int *rate8, int *dist8, macroblock_t * currMB )
#else
static double y_coding_block( int block8x8,int submode,int *rate8, macroblock_t * currMB )
#endif
{
    int j;
    int d8;
    double rdcost8;
    int temp_cbp;
    int rate_tmp;

    currMB->cbp  = 0;
    currMB->transform_16x16 = 0;
    currMB->transform_4x4[block8x8] = 0;
    *rate8=0;

    for ( j=0; j<8; j++ )
    {
        memcpy( img->b8_residual[j], &img->y_residual[j][0], 8*sizeof( int ) ); //xy_110506
    }
    //  8x8
    //store_coding_state(cs_abt8in4);

    if( submode==SUBBLOCK_INTER8x8 )   //RDO for 8x8 all 0 coeff, check if all 0 is better
    {
        int b8_y = 8 * ( block8x8 / 2 );
        int b8_x = 8 * ( block8x8 % 2 );
        prob_context_t eob_ctx = qcoder->cx_eob_flag_8x8[0];
        int lps_interval = eob_ctx.prob_fsm[eob_ctx.state].lps_interval;
        int interval = qcoder -> interval;
        int mps_interval = interval - lps_interval;
        int mps = eob_ctx.mps;
        int d8_all0 = 0;
        double rate8_all0, cost8_all0;
        double cost8;
        int x,y;

        if ( ( mps && lps_interval<mps_interval ) || ( !mps && lps_interval>mps_interval ) )
        {
            rate8_all0 = log( ( double )interval/( double )mps_interval ) / log( 2.0 );
        }
        else
        {
            rate8_all0 = log( ( double )interval/( double )lps_interval ) / log( 2.0 );
        }
        //  get distortion 8x8 all0
        for ( y = 0; y < B8_SIZE; y++ )
        {
            for ( x = 0; x < B8_SIZE; x++ )
            {
                d8_all0 += img->quad[img->b8_residual[y][x]];
            }
        }
        cost8_all0 = d8_all0 + img->lambda_mode * ( rate8_all0 );
        transform_b8( img->b8_residual );
        scanquant_b8( currMB->qp, ( submode==SUBBLOCK_INTRA8x8||submode==SUBBLOCK_INTRA4x4 ), block8x8, img->b8_residual, &( currMB->cbp ), &temp_cbp );

        *rate8 = write_block_coeff8x8( block8x8 );
        //  get distortion 8x8
        d8 = get_recon_distortion_8x8( block8x8 );
        cost8 = d8 + img->lambda_mode * ( *rate8 );
        if ( cost8_all0 < cost8 )
        {
            for ( y = 0; y < B8_SIZE; y++ )
            {
                for ( x = 0; x < B8_SIZE; x++ )
                {
                    p_img_y_rec[img->pix_y+b8_y+y][img->pix_x+b8_x+x] = inter_pred[b8_y+y][b8_x+x];
                }
            }
            img->cofAC[block8x8][0][0][0] = 0;
            *rate8 = write_block_coeff8x8( block8x8 );
        }
    }
    else
    {
        transform_b8( img->b8_residual );
        scanquant_b8( currMB->qp, ( submode==SUBBLOCK_INTRA8x8||submode==SUBBLOCK_INTRA4x4 ), block8x8, img->b8_residual, &( currMB->cbp ), &temp_cbp );
#if INTRA_MODE_ADAPTIVE
        if( submode==SUBBLOCK_INTRA8x8 )
        {
            *rate8 += write_luma_intra_mode( currMB->b8mode[block8x8/2][block8x8%2],block8x8,0 );
        }
#endif
        *rate8 += write_block_coeff8x8( block8x8 );
        //  get distortion 8x8
        d8 = get_recon_distortion_8x8( block8x8 );
    }
    rate_tmp = *rate8 ;

#if RDO_STAT
    *dist8 = d8;
#endif

    rdcost8 = d8 + img->lambda_mode * rate_tmp;
    //reset_coding_state(cs_abt8in4);

    return rdcost8;
}


/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
#if RDO_STAT
static double y_coding_block8in4( int block8x8,int submode,int *rate8in4, int *dist8in4, macroblock_t * currMB )
#else
static double y_coding_block8in4( int block8x8,int submode,int *rate8in4, macroblock_t * currMB )
#endif
{
    int j, block4x4;
    int d8in4;
    double rdcost8in4;
    int ACLevel_4x4[4][17],ACRun_4x4[4][17];
    int rate8in4_tmp=0;
    currMB->cbp  = 0;
    currMB->transform_16x16 = 0;

    //store_coding_state(cs_abt8in4);
    for ( j=0; j<8; j++ )
    {
        memcpy( img->b8_residual[j], &img->y_residual[j][0], 8*sizeof( int ) );
    }
    //  set transform4x4
    currMB->transform_4x4[block8x8] = 1;
    //
    for( block4x4 = 0; block4x4<4; block4x4++ )
    {
        if( submode==SUBBLOCK_INTRA4x4 )
        {
            rate8in4_tmp += qcoder_encode_pred_direction( qcoder, INTRA );
        }
        else
        {
            if ( img->type == P_IMG )
            {
                rate8in4_tmp += qcoder_encode_pred_direction( qcoder, FW );
                rate8in4_tmp += qcoder_encode_mvd_x_num( qcoder, img->mvd_4x4[FW][block8x8][block4x4].x, NUM_MVD_CX_4x4_QCODER );
                rate8in4_tmp += qcoder_encode_mvd_y_num( qcoder, img->mvd_4x4[FW][block8x8][block4x4].y, NUM_MVD_CX_4x4_QCODER );
            }
            else
            {
                int direction = currMB->b4_ref[block8x8][block4x4];
                rate8in4_tmp += qcoder_encode_pred_direction( qcoder, direction );
                if( direction != BI )
                {
                    rate8in4_tmp += qcoder_encode_mvd_x_num( qcoder, img->mvd_4x4[direction][block8x8][block4x4].x, NUM_MVD_CX_4x4_QCODER );
                    rate8in4_tmp += qcoder_encode_mvd_y_num( qcoder, img->mvd_4x4[direction][block8x8][block4x4].y, NUM_MVD_CX_4x4_QCODER );
                }
                else
                {
                    rate8in4_tmp += qcoder_encode_mvd_x_num( qcoder, img->bi_mvd_4x4[block8x8][block4x4].fw_mvd.x, NUM_MVD_CX_4x4_QCODER );
                    rate8in4_tmp += qcoder_encode_mvd_y_num( qcoder, img->bi_mvd_4x4[block8x8][block4x4].fw_mvd.y, NUM_MVD_CX_4x4_QCODER );
                    rate8in4_tmp += qcoder_encode_mvd_x_num( qcoder, img->bi_mvd_4x4[block8x8][block4x4].bw_mvd.x, NUM_MVD_CX_4x4_QCODER );
                    rate8in4_tmp += qcoder_encode_mvd_y_num( qcoder, img->bi_mvd_4x4[block8x8][block4x4].bw_mvd.y, NUM_MVD_CX_4x4_QCODER );
                }
            }
        }

        trans_scanquant_b4( img->vbs_qp_4x4, ( submode==SUBBLOCK_INTRA8x8||submode==SUBBLOCK_INTRA4x4 )?4:0, block8x8, block4x4, img->b8_residual, ACLevel_4x4[block4x4], ACRun_4x4[block4x4] );

#if INTRA_MODE_ADAPTIVE
        if( submode==SUBBLOCK_INTRA4x4 )
            rate8in4_tmp += write_luma_intra_mode( currMB->b4mode[block8x8/2][block8x8%2][block4x4/2][block4x4%2],block8x8,block4x4 );
#endif

        rate8in4_tmp += write_block_coeff4x4( block8x8,block4x4 );
    }

    d8in4 = get_recon_distortion_8x8( block8x8 );

    rdcost8in4 = d8in4 + img->lambda_mode * rate8in4_tmp;

    *rate8in4 = rate8in4_tmp;

#if RDO_STAT
    *dist8in4 = d8in4;
#endif

    return rdcost8in4;
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static int get_best_subblock_type( int block , macroblock_t *currMB )
{
    int rate, rate_overhead;
    int  rate8,rate8in4=0;
    double rdcost,min_rdcost=0;
    int best_ref[4],best_ref_4x4[4][4];
    int i,j,x,y;
    int k,m;
    int tmp_ACLevel_4x4[4][17],tmp_ACRun_4x4[4][17];
    int tmp_ACLevel_8x8[65],tmp_ACRun_8x8[65];
    int block_y, block_x;
    int ref_idx;
    int tmp_dc,tmp_y_dc;
    int cost;
    int blk4x4;
    int tmp_p_img_y_rec[8][8];
    int b4mode[2][2] = {{0,0},{0,0}};
    mv_t intra_mv= {0,0,-1};

#if RDO_STAT
    int dist8=0;
    int dist8in4=0;
#endif

    block_y = block/2;
    block_x = block%2;

    store_coding_state( cs_sb );
#if RDO_STAT
    {
        printf( "=============================================SubBlock %3d=============================================\n", block );
    }
#endif

    //SUBBLOCK_INTRA8x8
    currMB->b8_ref[block/2][block%2]= INTRA;
    for ( y=0; y<B8_SIZE ; y++ )
    {
        for ( x=0; x<B8_SIZE; x++ )
        {
            img->y_residual[y][x] = img->org_y_data[B8_SIZE * ( block_y )+y][B8_SIZE * ( block_x )+x] - intra_pred[B8_SIZE * ( block_y )+y][B8_SIZE * ( block_x )+x];
        }
    }
    rate_overhead = qcoder_encode_subblocksize( qcoder, 0 );
    rate_overhead += qcoder_encode_pred_direction( qcoder, INTRA );

#if RDO_STAT
    min_rdcost = y_coding_block( block,SUBBLOCK_INTRA8x8,&rate8,&dist8,currMB );
#else
    min_rdcost = y_coding_block( block,SUBBLOCK_INTRA8x8,&rate8,currMB );
#endif

    rate8 += rate_overhead;
    min_rdcost += img->lambda_mode * rate_overhead;

#if RDO_STAT
    {
        int y,x;
        printf( "                SubIntra8x8, Rate: %6d, Distortion: %6d, Rdcost: %6.2f (Bits: Overhead: %3d, Res: %3d)\n", rate8, dist8, min_rdcost, rate_overhead, rate8-rate_overhead );

        for( y=0; y<8; y++ )
        {
            for( x=0; x<8; x++ )
            {
                printf( "%3d ", img->y_residual[y][x] );
            }
            printf( "\n" );
        }
        printf( "\n" );
    }
#endif
    for( i=0; i<65; i++ )
    {
        tmp_ACLevel_8x8[i] = img->cofAC[block][0][0][i];
        tmp_ACRun_8x8[i]   = img->cofAC[block][0][1][i];
    }
    for( j=0; j<8; j++ )
        for( i=0; i<8; i++ )
            tmp_p_img_y_rec[j][i]=p_img_y_rec[img->pix_y+block_y*8+j][img->pix_x+block_x*8+i];

    tmp_y_dc = currMB->y_dc[block_y][block_x];
    currMB->subblock_type[block] = SUBBLOCK_INTRA8x8;
    rate = rate8;
    if( input->AdaptiveRounding )
        set_qadjust_to_best1_y();

    store_coding_state( cs_sb_best );
    reset_coding_state( cs_sb );


    //SUBBLOCK_INTRA4x4
    if( input->ABTen && input->IABTen )
    {
        rate_overhead = qcoder_encode_subblocksize( qcoder, 1 );

#if RDO_STAT
        rdcost = y_coding_block8in4( block,SUBBLOCK_INTRA4x4,&rate8in4,&dist8in4,currMB );
#else
        rdcost = y_coding_block8in4( block,SUBBLOCK_INTRA4x4,&rate8in4,currMB );
#endif

        rate8in4 += rate_overhead;
        rdcost += img->lambda_mode * rate_overhead;

#if RDO_STAT
        {
            int x,y;
            printf( "                SubIntra4x4, Rate: %6d, Distortion: %6d, Rdcost: %6.2f (Bits: Overhead: %3d, Res: %3d)\n", rate8in4, dist8in4, rdcost, rate_overhead, rate8in4-rate_overhead );

            for( y=0; y<8; y++ )
            {
                for( x=0; x<8; x++ )
                {
                    printf( "%3d ", img->y_residual[y][x] );
                }
                printf( "\n" );
            }
            printf( "\n" );
        }
#endif

        if ( rdcost <  min_rdcost )  //  choose 4x4
        {
            min_rdcost = rdcost;
            currMB->subblock_type[block] = SUBBLOCK_INTRA4x4;

            if( input->AdaptiveRounding )
                set_qadjust_to_best1_y();
            for( i=0; i<4; i++ )
                for( j=0; j<17; j++ )
                {
                    tmp_ACLevel_4x4[i][j] = img->cofAC[block][i][0][j];
                    tmp_ACRun_4x4[i][j]   = img->cofAC[block][i][1][j];
                }
            for( j=0; j<8; j++ )
                for( i=0; i<8; i++ )
                    tmp_p_img_y_rec[j][i]=p_img_y_rec[img->pix_y+block_y*8+j][img->pix_x+block_x*8+i];
            //*cbp = cbp_4x4;
            //*cbp_blk = cbp_blk_4x4;
            rate = rate8in4;
            tmp_dc = 0;
            for ( k=0; k<2; k++ )
                for ( m=0; m<2; m++ )
                    tmp_dc += currMB->y_4x4_dc[block_y][block_x][k][m];
            currMB->y_dc[block_y][block_x] = ( tmp_dc+1 )/2; //(tmp_dc+2)/4;
            tmp_y_dc = currMB->y_dc[block_y][block_x];

            for( j=0; j<2; j++ )
                for( i=0; i<2; i++ )
                    b4mode[j][i] = currMB->b4mode[block/2][block%2][j][i];

            store_coding_state( cs_sb_best );
        }
        reset_coding_state( cs_sb );

    }//end if ABT

    //SUBBLOCK_INTER8x8
    best_ref[block] = get_best_ref_and_cost( &cost, INTER8x8, block );
    //best8x8ref[INTER8x8][block/2][block%2] = best_ref[block];
    currMB->b8_ref[block/2][block%2]= best_ref[block];
    get_inter_pred_y_block( block,INTER8x8, currMB );
    for ( y=0; y<B8_SIZE ; y++ )
    {
        for ( x=0; x<B8_SIZE; x++ )
        {
            img->y_residual[y][x] = img->org_y_data[B8_SIZE * ( block_y )+y][B8_SIZE * ( block_x )+x] - inter_pred[B8_SIZE * ( block_y )+y][B8_SIZE * ( block_x )+x];
        }
    }
    rate_overhead = qcoder_encode_subblocksize( qcoder, 0 );


    //rate_overhead = qcoder_encode_subblocksize(qcoder, 0);
    //rate_overhead += qcoder_encode_pred_direction(qcoder, currMB->b8_ref[block_y][block_x]);
    //rate_overhead += write_block_mvd(currMB,8,block,0,1);


    if( img->type == P_IMG )
    {
        rate_overhead += qcoder_encode_pred_direction( qcoder, FW );
        //rate_overhead += qcoder_encode_mvd_x(qcoder, img->mvd[FW][INTER8x8][block>>1][block%2].x);
        //rate_overhead += qcoder_encode_mvd_y(qcoder, img->mvd[FW][INTER8x8][block>>1][block%2].y);
        rate_overhead += qcoder_encode_mvd_x_num( qcoder, img->mvd[FW][INTER8x8][block>>1][block%2].x, NUM_MVD_CX_8x8_QCODER );
        rate_overhead += qcoder_encode_mvd_y_num( qcoder, img->mvd[FW][INTER8x8][block>>1][block%2].y, NUM_MVD_CX_8x8_QCODER );
    }
    else
    {
        int direction = best_ref[block];
        rate_overhead += qcoder_encode_pred_direction( qcoder, direction );
        if( direction != BI )
        {
            //rate_overhead += qcoder_encode_mvd_x(qcoder, img->mvd[direction][INTER8x8][block>>1][block%2].x);
            //rate_overhead += qcoder_encode_mvd_y(qcoder, img->mvd[direction][INTER8x8][block>>1][block%2].y);
            rate_overhead += qcoder_encode_mvd_x_num( qcoder, img->mvd[direction][INTER8x8][block>>1][block%2].x, NUM_MVD_CX_8x8_QCODER );
            rate_overhead += qcoder_encode_mvd_y_num( qcoder, img->mvd[direction][INTER8x8][block>>1][block%2].y, NUM_MVD_CX_8x8_QCODER );
        }
        else
        {
            //rate_overhead += qcoder_encode_mvd_x(qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].fw_mvd.x);
            //rate_overhead += qcoder_encode_mvd_y(qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].fw_mvd.y);
            //rate_overhead += qcoder_encode_mvd_x(qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].bw_mvd.x);
            //rate_overhead += qcoder_encode_mvd_y(qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].bw_mvd.y);
            rate_overhead += qcoder_encode_mvd_x_num( qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].fw_mvd.x, NUM_MVD_CX_8x8_QCODER );
            rate_overhead += qcoder_encode_mvd_y_num( qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].fw_mvd.y, NUM_MVD_CX_8x8_QCODER );
            rate_overhead += qcoder_encode_mvd_x_num( qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].bw_mvd.x, NUM_MVD_CX_8x8_QCODER );
            rate_overhead += qcoder_encode_mvd_y_num( qcoder, img->bi_mvd[INTER8x8][block>>1][block%2].bw_mvd.y, NUM_MVD_CX_8x8_QCODER );
        }
    }

#if RDO_STAT
    rdcost = y_coding_block( block,SUBBLOCK_INTER8x8,&rate8,&dist8,currMB );
#else
    rdcost = y_coding_block( block,SUBBLOCK_INTER8x8,&rate8,currMB );
#endif

    rate8 += rate_overhead;
    rdcost += img->lambda_mode * rate_overhead;

#if RDO_STAT
    {
        int x, y;
        mv_t tmp_mv = img->pred_mv[FW][INTER8x8][block_y][block_x];
        printf( "                SubInter8x8, Rate: %6d, Distortion: %6d, Rdcost: %6.2f, PMV:(%3d,%3d), MV:(%3d,%3d), (Bits: Overhead: %3d, Res: %3d)\n", rate8, dist8, rdcost, tmp_mv.x, tmp_mv.y,img->mv[FW][INTER8x8][block_y][block_x].x,img->mv[FW][INTER8x8][block_y][block_x].y, rate_overhead, rate8 - rate_overhead );

        for( y=0; y<8; y++ )
        {
            for( x=0; x<8; x++ )
            {
                printf( "%3d ", img->y_residual[y][x] );
            }
            printf( "\n" );
        }
        printf( "\n" );
    }
#endif

    if ( rdcost<  min_rdcost )  //  choose 8x8
    {
        min_rdcost = rdcost;
        currMB->subblock_type[block] = SUBBLOCK_INTER8x8;
        if( input->AdaptiveRounding )
            set_qadjust_to_best1_y();
        for( i=0; i<65; i++ )
        {
            tmp_ACLevel_8x8[i] = img->cofAC[block][0][0][i];
            tmp_ACRun_8x8[i]   = img->cofAC[block][0][1][i];
        }
        for( j=0; j<8; j++ )
            for( i=0; i<8; i++ )
                tmp_p_img_y_rec[j][i]=p_img_y_rec[img->pix_y+block_y*8+j][img->pix_x+block_x*8+i];
        rate = rate8;
        tmp_y_dc = currMB->y_dc[block_y][block_x];

        store_coding_state( cs_sb_best );
    }

    reset_coding_state( cs_sb );

    //SUBBLOCK_INTER4x4
    if( input->ABTen && input->InterABTen )
    {
        for( blk4x4=0; blk4x4<4; blk4x4++ )
        {
            best_ref_4x4[block][blk4x4] = get_best_ref_and_cost( &cost, INTER4x4, 4*block+blk4x4 );
            currMB->b4_ref[block][blk4x4] = best_ref_4x4[block][blk4x4];
            if ( img->type == P_IMG )
                frm_mv_4x4[MAX_REF_NUM][img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2] = img->mv_4x4[currMB->b4_ref[block][blk4x4]][block][blk4x4];
            else if( img->type == B_IMG )
            {
                if ( currMB->b4_ref[block][blk4x4] == FW )
                {
                    frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].fw_mv = img->mv_4x4[FW][block][blk4x4];
                    frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].bw_mv = intra_mv;
                }
                else if ( currMB->b4_ref[block][blk4x4] == BW )
                {
                    frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].fw_mv = intra_mv;
                    frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].bw_mv = img->mv_4x4[BW][block][blk4x4];
                }
                else
                {
                    frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2] = img->bi_mv_4x4[block][blk4x4];
                }
            }

        }
        get_inter_pred_y_block( block,INTER4x4,currMB );

        for ( y=0; y<B8_SIZE ; y++ )
        {
            for ( x=0; x<B8_SIZE; x++ )
            {
                img->y_residual[y][x] = img->org_y_data[B8_SIZE * ( block_y )+y][B8_SIZE * ( block_x )+x] - inter_pred[B8_SIZE * ( block_y )+y][B8_SIZE * ( block_x )+x];
            }
        }
        rate_overhead = qcoder_encode_subblocksize( qcoder, 1 );

#if RDO_STAT
        rdcost = y_coding_block8in4( block,SUBBLOCK_INTER4x4,&rate8in4,&dist8in4,currMB );
#else
        rdcost = y_coding_block8in4( block,SUBBLOCK_INTER4x4,&rate8in4,currMB );
#endif

        rate8in4 += rate_overhead;
        rdcost += img->lambda_mode * rate_overhead;

#if RDO_STAT
        {
            int x, y;
            printf( "                SubInter4x4, Rate: %6d, Distortion: %6d, Rdcost: %6.2f,(Bits: Overhead: %3d, Res: %3d) \n\n", rate8in4, dist8in4, rdcost, rate_overhead, rate8in4-rate_overhead );
            for( y=0; y<8; y++ )
            {
                for( x=0; x<8; x++ )
                {
                    printf( "%3d ", img->y_residual[y][x] );
                }
                printf( "\n" );
            }
            printf( "\n" );
        }
#endif

        if ( rdcost <  min_rdcost )  //  choose 4x4
        {
            min_rdcost = rdcost;
            currMB->subblock_type[block] = SUBBLOCK_INTER4x4;
            if( input->AdaptiveRounding )
                set_qadjust_to_best1_y();
            for( i=0; i<4; i++ )
                for( j=0; j<17; j++ )
                {
                    tmp_ACLevel_4x4[i][j] = img->cofAC[block][i][0][j];
                    tmp_ACRun_4x4[i][j]   = img->cofAC[block][i][1][j];
                }
            for( j=0; j<8; j++ )
                for( i=0; i<8; i++ )
                    tmp_p_img_y_rec[j][i]=p_img_y_rec[img->pix_y+block_y*8+j][img->pix_x+block_x*8+i];
            currMB->transform_4x4[block] = 1;
            rate = rate8in4;
            tmp_dc = 0;
            for ( k=0; k<2; k++ )
                for ( m=0; m<2; m++ )
                    tmp_dc += currMB->y_4x4_dc[block_y][block_x][k][m];
            currMB->y_dc[block_y][block_x] = ( tmp_dc+1 )/2; //(tmp_dc+2)/4;
            tmp_y_dc = currMB->y_dc[block_y][block_x];

            store_coding_state( cs_sb_best );
        }
    }
    reset_coding_state( cs_sb_best );

    //set
    for( j=0; j<8; j++ )
        for( i=0; i<8; i++ )
            p_img_y_rec[img->pix_y+block_y*8+j][img->pix_x+block_x*8+i] =  tmp_p_img_y_rec[j][i];

    if( currMB->subblock_type[block] == SUBBLOCK_INTER4x4 || currMB->subblock_type[block] == SUBBLOCK_INTRA4x4 )
    {
        for( i=0; i<4; i++ )
            for( j=0; j<17; j++ )
            {
                img->cofAC[block][i][0][j] = tmp_ACLevel_4x4[i][j] ;
                img->cofAC[block][i][1][j] = tmp_ACRun_4x4[i][j];
            }
        currMB->transform_4x4[block] = 1;
        if( currMB->subblock_type[block] == SUBBLOCK_INTER4x4 )
        {
            for ( blk4x4 = 0; blk4x4 < 4; blk4x4++ )
            {
                ref_idx = currMB->b4_ref[block][blk4x4];
                // frm_mv_4x4[MAX_REF_NUM][img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2] = img->mv_4x4[ref_idx][block][blk4x4];

                if ( img->type == P_IMG )
                    frm_mv_4x4[MAX_REF_NUM][img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2] = img->mv_4x4[currMB->b4_ref[block][blk4x4]][block][blk4x4];
                else if( img->type == B_IMG )
                {
                    if ( currMB->b4_ref[block][blk4x4] == FW )
                    {
                        frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].fw_mv = img->mv_4x4[FW][block][blk4x4];
                        frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].bw_mv = intra_mv;
                    }
                    else if ( currMB->b4_ref[block][blk4x4] == BW )
                    {
                        frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].fw_mv = intra_mv;
                        frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2].bw_mv = img->mv_4x4[BW][block][blk4x4];
                    }
                    else
                    {
                        frm_bi_mv_4x4[img->block8_y + block_y][img->block8_x + block_x][blk4x4/2][blk4x4%2] = img->bi_mv_4x4[block][blk4x4];
                    }
                }

                if ( ref_idx != BI ) //single direction
                {
                    currMB->mvd_4x4[block][blk4x4] = img->mvd_4x4[ref_idx][block][blk4x4];
                }
                else
                {
                    currMB->bi_mvd_4x4[block][blk4x4] = img->bi_mvd_4x4[block][blk4x4];
                }
            }
            currMB->y_dc[block_y][block_x] = 0;
        }
        else
        {
            for( j=0; j<2; j++ )
                for( i=0; i<2; i++ )
                {
                    currMB->b4mode[block/2][block%2][j][i] = b4mode[j][i];
                    currMB->b4_ref[block][j*2+i] = BI;
                }
            currMB->y_dc[block_y][block_x] = tmp_y_dc;
            currMB->b8_ref[block_y][block_x] = BI;// add by xy_110911
        }
    }
    else
    {
        for( i=0; i<65; i++ )
        {
            img->cofAC[block][0][0][i] = tmp_ACLevel_8x8[i];
            img->cofAC[block][0][1][i] = tmp_ACRun_8x8[i];
        }
        currMB->transform_4x4[block] = 0;
        if( currMB->subblock_type[block] == SUBBLOCK_INTER8x8 )
        {

            ref_idx = currMB->b8_ref[block_y][block_x];
            //frm_mv[MAX_REF_NUM][img->block8_y + block_y][img->block8_x + block_x] = img->mv[ref_idx][INTER8x8][block_y][block_x];

            if ( img->type == P_IMG )
                frm_mv[MAX_REF_NUM][img->block8_y + block_y][img->block8_x + block_x] = img->mv[ref_idx][INTER8x8][block_y][block_x];
            else if( img->type == B_IMG )
            {
                if ( currMB->b8_ref[block_y][block_x] == FW )
                {
                    frm_bi_mv[img->block8_y + block_y][img->block8_x + block_x].fw_mv = img->mv[FW][INTER8x8][block_y][block_x];
                    frm_bi_mv[img->block8_y + block_y][img->block8_x + block_x].bw_mv = intra_mv;
                }
                else if ( currMB->b8_ref[block_y][block_x] == BW )
                {
                    frm_bi_mv[img->block8_y + block_y][img->block8_x + block_x].fw_mv = intra_mv;
                    frm_bi_mv[img->block8_y + block_y][img->block8_x + block_x].bw_mv = img->mv[BW][INTER8x8][block_y][block_x];
                }
                else
                {
                    frm_bi_mv[img->block8_y + block_y][img->block8_x + block_x] = img->bi_mv[INTER8x8][block_y][block_x];
                }
            }


            if ( ref_idx != BI ) //single direction
            {
                currMB->mvd[block_y][block_x] = img->mvd[ref_idx][INTER8x8][block_y][block_x];
            }
            else
            {
                currMB->bi_mvd[block_y][block_x] = img->bi_mvd[INTER8x8][block_y][block_x];
            }
            currMB->y_dc[block_y][block_x] = 0;
        }
        else
        {
            currMB->y_dc[block_y][block_x] = tmp_y_dc;
            currMB->b8_ref[block_y][block_x] = BI;// add by xy_110911
        }
    }
    if( input->AdaptiveRounding )
        set_best1_to_qadjust();
    return rate;

}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static double rdcost_for_macroblocks( int mode )
{
    extern int best8x8ref[MAX_MODE_NUM][2][2];
    int r, d;
    int j,i,uv;
    double rdcost=0;

    int best_ref[4];
    int cost;
    int block;
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];

    store_coding_state( cs_mb );

    if ( mode == I8MB ) //I frame   for B,P frame, intra mode refers to subblock_type
    {
        set_mode_and_ref( mode, currMB );
        rdcost = get_intra_rdcost();
    }
    else
    {
        if ( mode == SKIP )
        {
            find_skip_mv();
            set_mode_and_ref( mode, currMB );
            get_inter_pred_y ( currMB );

            for( j=0; j<16; j++ )
                for( i=0; i<16; i++ )
                    p_img_y_rec[img->pix_y+j][img->pix_x+i]=inter_pred[j][i];

            for ( uv = 0; uv < UV; uv++ )
            {
                get_inter_pred_uv ( currMB, uv );
                for( j=0; j<8; j++ )
                    for( i=0; i<8; i++ )
                        p_img_uv_rec[uv][img->pix_c_y+j][img->pix_c_x+i]=inter_pred[j][i];
            }
            d = get_recon_distortion();
            r = write_skip_flag();
            rdcost = d + img->lambda_mode * r;

#if RDO_STAT
            {
                mv_t tmp_mv = img->pred_mv[FW][SKIP][0][0];
                printf( "\n                Rate: %6d, Distortion: %6d, Rdcost: %6.2f, MV:(%3d,%3d)\n", r, d, rdcost, tmp_mv.x, tmp_mv.y );
            }
#endif
        }
        else
        {
            if( mode == INTER16x16 )
            {
#if RDO_STAT
                int bits[6];
#endif
                best_ref[0] = get_best_ref_and_cost( &cost, mode, 0 );
                for ( j=0; j<2; j++ )
                    for( i=0; i<2; i++ )
                        best8x8ref[mode][j][i] = best_ref[0];

                set_mode_and_ref( mode, currMB );

#if RDO_STAT
                bits[0] = write_skip_flag();
                bits[1] = qcoder_encode_blocksize( qcoder, 1 );
                bits[2] = qcoder_encode_pred_direction( qcoder, currMB->b8_ref[0][0] );
                bits[3] = write_block_mvd( currMB, 16, 0, 0, 1 );

                bits[4] = inter_y_residual_coding  ( NON_ABT );
                bits[5] = inter_uv_residual_coding ( );

                r = bits[0] + bits[1] + bits[2] + bits[3] + bits[4] + bits[5];
#else
                r = write_skip_flag();
                r += qcoder_encode_blocksize( qcoder, 1 );
                r += qcoder_encode_pred_direction( qcoder, currMB->b8_ref[0][0] );
                r += write_block_mvd( currMB, 16, 0, 0, 1 );
                r += inter_y_residual_coding  ( NON_ABT );
                r += inter_uv_residual_coding ( );
#endif
                d  = get_recon_distortion     ( );
                rdcost = d + img->lambda_mode * r;

#if RDO_STAT
                {
                    int x,y;
                    mv_t tmp_mv = img->pred_mv[FW][INTER16x16][0][0];
                    printf( "                Rate: %6d, Distortion: %6d, Rdcost: %6.2f, (Bits: Skip Flag: %3d, Blocksize: %3d, PredDirect: %3d, MVD: %3d, ResY: %3d, ResUV: %3d) PMV:(%3d,%3d), MV:(%3d,%3d)\n", r, d, rdcost, bits[0], bits[1], bits[2], bits[3], bits[04], bits[5], tmp_mv.x, tmp_mv.y, img->mv[FW][INTER16x16][0][0].x, img->mv[FW][INTER16x16][0][0].y );

                    for ( y = 0; y < 16; y++ )
                    {
                        for ( x = 0; x < 16; x++ )
                        {
                            printf( "%3d, ", img->y_residual[y][x] );
                        }
                        printf( "\n" );
                    }
                }
#endif
            }
            else if( mode == INTER8x8 )
            {

#if RDO_STAT
                int bits[4];
#endif
                currMB->mb_type = mode;

#if RDO_STAT
                bits[0] = write_skip_flag();
                bits[1] = qcoder_encode_blocksize( qcoder, 0 );
                //store_coding_state(cs_abt);  //
                bits[2] = 0;
                for( block=0; block<4; block++ )
                {
                    bits[2] += get_best_subblock_type( block , currMB );
                }
                bits[3] = inter_uv_residual_coding ( );
                r = bits[0] + bits[1] + bits[2] + bits[3];
#else
                r = write_skip_flag();
                r += qcoder_encode_blocksize( qcoder, 0 );
                for( block=0; block<4; block++ )
                {
                    r += get_best_subblock_type( block , currMB );
                }
                r += inter_uv_residual_coding ( );
#endif
                d  = get_recon_distortion     ( );
                rdcost = d + img->lambda_mode * r;

#if RDO_STAT
                {
                    printf( "                Rate: %6d, Distortion: %6d, Rdcost: %6.2f, (Bits: Skip Flag: %3d, Blocksize: %3d, SubblockY: %3d, ResUV: %3d)\n", r, d, rdcost,bits[0], bits[1], bits[2], bits[3] );
                }
#endif
            }
        }
    }
    reset_coding_state( cs_mb );
    return rdcost;
}


/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static void store_mb_para()
{
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];
    int i, j,k,m;
    best_parameter.best_cbp  = currMB->cbp;
    best_parameter.best_cbp_16x16  = currMB->cbp_16x16;
    best_parameter.best_transform_16x16 = currMB->transform_16x16;
    for ( i=0; i<4; i++ )
    {
        best_parameter.best_cbp_4x4[i]  = currMB->cbp_4x4[i];
        best_parameter.best_transform_4x4[i] = currMB->transform_4x4[i];
        best_parameter.best_subblock_type[i] = currMB->subblock_type[i];
    }
    best_parameter.best_mode = currMB->mb_type;
    memcpy( best_parameter.b8_ref, currMB->b8_ref, 4*sizeof( int ) );
    for ( j = 0; j < 2; j++ )
    {
        for ( i = 0; i < 2; i++ )
        {
            best_parameter.mvd[j][i]    = currMB->mvd[j][i];
            best_parameter.bi_mvd[j][i] = currMB->bi_mvd[j][i];
        }
    }
    memcpy( best_parameter.b4_ref, currMB->b4_ref, 16*sizeof( int ) );
    for ( j = 0; j < 4; j++ )
    {
        for ( i = 0; i < 4; i++ )
        {
            best_parameter.mvd_4x4[j][i]    = currMB->mvd_4x4[j][i];
            best_parameter.bi_mvd_4x4[j][i] = currMB->bi_mvd_4x4[j][i];
        }
    }

    for( i=0; i<8; i++ )
        for( j=0; j<4; j++ )
            for( k=0; k<2; k++ )
                for( m=0; m<65; m++ )
                    best_parameter.cofAC[i][j][k][m]=img->cofAC[i][j][k][m];

    for ( i=0; i<257; i++ )
        for ( j=0; j<2; j++ )
            best_parameter.cofAC16x16[i][j] = img->cofAC16x16[i][j];

    for( j=0; j<16; j++ )
        for( i=0; i<16; i++ )
            best_parameter.recY[j][i]=p_img_y_rec[img->pix_y+j][img->pix_x+i];

    for( m=0; m<2; m++ )
        for( j=0; j<8; j++ )
            for( i=0; i<8; i++ )
                best_parameter.recUV[m][j][i]=p_img_uv_rec[m][img->pix_c_y+j][img->pix_c_x+i];
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static void set_stored_mb_para()
{
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];
    int i, j,k,m;
    currMB->cbp     = best_parameter.best_cbp;
    currMB->cbp_16x16 = best_parameter.best_cbp_16x16;
    currMB->transform_16x16 = best_parameter.best_transform_16x16;
    for ( i=0; i<4; i++ )
    {
        currMB->cbp_4x4[i] = best_parameter.best_cbp_4x4[i];
        currMB->transform_4x4[i] = best_parameter.best_transform_4x4[i];
        currMB->subblock_type[i] = best_parameter.best_subblock_type[i];
    }
    currMB->mb_type = best_parameter.best_mode;
    memcpy( currMB->b8_ref, best_parameter.b8_ref, 4*sizeof( int ) );
    for ( j = 0; j < 2; j++ )
    {
        for ( i = 0; i < 2; i++ )
        {
            currMB->mvd[j][i]    = best_parameter.mvd[j][i];
            currMB->bi_mvd[j][i] = best_parameter.bi_mvd[j][i];
        }
    }
    memcpy( currMB->b4_ref, best_parameter.b4_ref,  16*sizeof( int ) );
    for ( j = 0; j < 4; j++ )
    {
        for ( i = 0; i < 4; i++ )
        {
            currMB->mvd_4x4[j][i]    = best_parameter.mvd_4x4[j][i];
            currMB->bi_mvd_4x4[j][i] = best_parameter.bi_mvd_4x4[j][i] ;
        }
    }

    //coefficient
    for( i=0; i<8; i++ )
        for( j=0; j<4; j++ )
            for( k=0; k<2; k++ )
                for( m=0; m<65; m++ )
                    img->cofAC[i][j][k][m]=best_parameter.cofAC[i][j][k][m];

    for ( i=0; i<257; i++ )
        for ( j=0; j<2; j++ )
            img->cofAC16x16[i][j] = best_parameter.cofAC16x16[i][j];

    for( j=0; j<16; j++ )
        for( i=0; i<16; i++ )
            p_img_y_rec[img->pix_y+j][img->pix_x+i]=best_parameter.recY[j][i];

    for( m=0; m<2; m++ )
        for( j=0; j<8; j++ )
            for( i=0; i<8; i++ )
                p_img_uv_rec[m][img->pix_c_y+j][img->pix_c_x+i]=best_parameter.recUV[m][j][i];
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static int get_best_ref_and_cost( int *p_cost, int mode, int block )
{
    int ref_idx, max_ref, cost, best_ref=0;
    int min_cost        = MAX_VALUE;
    int b_frame         = ( img->type == B_IMG );

    int block_y=0,block_x=0;
    int blk4x4_y=0,blk4x4_x=0;
    int pic_block_x    ;
    int pic_block_y    ;
    int pic_pix_x      ;
    int pic_pix_y      ;

    int tmp_time;
    time_t ltime1;
    time_t ltime2;
#if defined(WIN32) && !defined(__MINGW32__)
    struct _timeb tstruct1;
    struct _timeb tstruct2;
#else
    struct timeb tstruct1;
    struct timeb tstruct2;
#endif

    ftime( &tstruct1 );
    time( &ltime1 );

    if ( mode==INTER4x4 )
    {
        block_y  = block/8;
        block_x  = ( block%8 )/4;
        blk4x4_y = ( ( block%8 )%4 )/2;
        blk4x4_x = ( ( block%8 )%4 )%2;

        pic_block_x     = img->block8_x + block_x;
        pic_block_y     = img->block8_y + block_y;
        pic_pix_x       = ( pic_block_x << 3 ) + ( blk4x4_x << 2 );
        pic_pix_y       = ( pic_block_y << 3 ) + ( blk4x4_y << 2 );
    }
    else
    {
        pic_block_x     = img->block8_x + blk_offset_x[mode][block];
        pic_block_y     = img->block8_y + blk_offset_y[mode][block];
        pic_pix_x       = pic_block_x << 3;
        pic_pix_y       = pic_block_y << 3;
    }

    if ( b_frame )
    {
        max_ref = 2;
    }
    else
    {
        // max_ref = RFM_MIN(img->ref_num, input->ref_num);
        max_ref = 1; //xuyan_ref
    }

#ifdef FastME
    if ( input->UseFME )
        bipred_flag = 0;
#endif

    for ( ref_idx = 0; ref_idx < max_ref; ref_idx++ )
    {
        if ( mode==INTER4x4 )
            cost = blk4x4_me( ref_idx, pic_pix_x, pic_pix_y, mode, block_y, block_x, blk4x4_y, blk4x4_x );
        else
            cost = me( ref_idx, pic_pix_x, pic_pix_y, mode );
        if ( cost < min_cost )
        {
            min_cost = cost;
            best_ref = ref_idx;
        }
    }

    if ( b_frame )
    {
#ifdef FastME
        if ( input->UseFME )
            bipred_flag = 0;
#endif

        if ( mode==INTER4x4 )
            cost = blk4x4_me_bid( 2, pic_pix_x, pic_pix_y, mode, block_y, block_x, blk4x4_y, blk4x4_x );
        else
            cost = me_bid( 2, pic_pix_x, pic_pix_y, mode );

        if ( cost < min_cost )
        {
            min_cost = cost;
            best_ref = 2;
        }
    }
    *p_cost += min_cost;

    time( &ltime2 );
    ftime( &tstruct2 );
    tmp_time = ( ltime2 * 1000 + tstruct2.millitm ) - ( ltime1 * 1000 + tstruct1.millitm );
    tot_metime = tot_metime + tmp_time;

    return best_ref;
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
void get_residual( int mode, byte **org, byte **pred, int **residual )
{
    int y, x;
    for ( y=0; y<input->blk_size[mode][1]; y++ )
    {
        for ( x=0; x<input->blk_size[mode][0]; x++ )
        {
            residual[y][x] = org[y][x] - pred[y][x];
        }
    }
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static void set_final_mv_ref()
{
    static const mv_t intra_mv = {0, 0, -1};

    int block_y = img->block8_y;
    int block_x = img->block8_x;
    int i, j, ref_idx;
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];
    best_mode = currMB->mb_type;
    if ( best_mode == I8MB )
    {
        switch( img->type )
        {
            case P_IMG:
                for ( j=0; j<2; j++ )
                {
                    for ( i=0; i<2; i++ )
                    {
                        frm_mv[MAX_REF_NUM][block_y + j][block_x + i] = intra_mv;
                    }
                }
                if( input->UseFME == 1 )
                {
                    for ( j=0; j<4; j++ )
                    {
                        for ( i=0; i<4; i++ )
                        {
                            curr_frm_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = intra_mv;
                        }
                    }
                }
                break;
            case B_IMG:
                for ( j=0; j<2; j++ )
                {
                    for ( i=0; i<2; i++ )
                    {
                        frm_bi_mv[block_y + j][block_x + i].fw_mv = intra_mv;
                        frm_bi_mv[block_y + j][block_x + i].bw_mv = intra_mv;
                    }
                }
                if( input->UseFME == 1 )
                {
                    for ( j=0; j<4; j++ )
                    {
                        for ( i=0; i<4; i++ )
                        {
                            curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = intra_mv;
                            curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = intra_mv;
                        }
                    }
                }
                break;
        }
    }
    else
    {
        switch( img->type )
        {
            case P_IMG:
                if( input->ABTen && input->InterABTen )
                {
                    for ( j=0; j<4; j++ )
                    {
                        if( best_mode == INTER8x8 && currMB->subblock_type[j] == SUBBLOCK_INTER4x4 )
                        {
                            for ( i=0; i<4; i++ )
                            {
                                ref_idx = currMB->b4_ref[j][i];
                                frm_mv_4x4[MAX_REF_NUM][block_y + j/2][block_x + j%2][i/2][i%2] = img->mv_4x4[ref_idx][j][i];
                            }
                        }
                        else
                        {
                            ref_idx = currMB->b8_ref[j/2][j%2];
                            frm_mv[MAX_REF_NUM][block_y + j/2][block_x + j%2] = img->mv[ref_idx][best_mode][j/2][j%2];

                        }
                    }
                }
                else
                {
                    for ( j=0; j<2; j++ )
                    {
                        for ( i=0; i<2; i++ )
                        {
                            ref_idx = currMB->b8_ref[j][i];
                            frm_mv[MAX_REF_NUM][block_y + j][block_x + i] = img->mv[ref_idx][best_mode][j][i];
                        }
                    }
                }
#ifdef FastME
                if( input->UseFME == 1 )
                {
                    if( input->ABTen && input->InterABTen )
                    {
                        for ( j=0; j<4; j++ )
                        {
                            if( best_mode == INTER8x8 && currMB->subblock_type[j] == SUBBLOCK_INTER4x4 )
                            {
                                for ( i=0; i<4; i++ )
                                {
                                    ref_idx = currMB->b4_ref[j][i];
                                    curr_frm_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->mv_4x4[ref_idx][j][i];
                                }
                            }
                            else
                            {
                                ref_idx = currMB->b8_ref[j/2][j%2];
                                for ( i=0; i<4; i++ )
                                    curr_frm_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->mv[ref_idx][best_mode][j/2][j%2];
                            }
                        }
                    }
                    else
                    {
                        for ( j=0; j<4; j++ )
                        {
                            ref_idx = currMB->b8_ref[j/2][j%2];
                            for ( i = 0; i<4; i++ )
                            {
                                curr_frm_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->mv[ref_idx][best_mode][j/2][j%2];
                            }
                        }
                    }
                }
#endif
                break;
            case B_IMG:
                if( input->ABTen && input->InterABTen )
                {
                    for ( j=0; j<4; j++ )
                    {
                        if( best_mode == INTER8x8 && currMB->subblock_type[j] == SUBBLOCK_INTER4x4 )
                        {

                            for ( i=0; i<4; i++ )
                            {
                                if ( currMB->b4_ref[j][i] == FW )
                                {
                                    frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = img->mv_4x4[FW][j][i];
                                    frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = intra_mv;
                                }
                                else if ( currMB->b4_ref[j][i] == BW )
                                {
                                    frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = intra_mv;
                                    frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = img->mv_4x4[BW][j][i];
                                }
                                else
                                {
                                    frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->bi_mv_4x4[j][i];
                                }

                                ref_idx = currMB->b4_ref[j][i];
                                frm_mv_4x4[MAX_REF_NUM][block_y + j/2][block_x + j%2][i/2][i%2] = img->mv_4x4[ref_idx][j][i];
                            }
                        }
                        else
                        {
                            if ( currMB->b8_ref[j/2][j%2] == FW )
                            {
                                frm_bi_mv[block_y + j/2][block_x + j%2].fw_mv = img->mv[FW][best_mode][j/2][j%2];
                                frm_bi_mv[block_y + j/2][block_x + j%2].bw_mv = intra_mv;
                            }
                            else if ( currMB->b8_ref[j/2][j%2] == BW )
                            {
                                frm_bi_mv[block_y + j/2][block_x + j%2].fw_mv = intra_mv;
                                frm_bi_mv[block_y + j/2][block_x + j%2].bw_mv = img->mv[BW][best_mode][j/2][j%2];
                            }
                            else
                            {
                                frm_bi_mv[block_y + j/2][block_x + j%2] = img->bi_mv[best_mode][j/2][j%2];
                            }
                        }
                    }
                }
                else
                {
                    for ( j=0; j<2; j++ )
                    {
                        for ( i=0; i<2; i++ )
                        {
                            if ( currMB->b8_ref[j][i] == FW )
                            {
                                frm_bi_mv[block_y + j][block_x + i].fw_mv = img->mv[FW][best_mode][j][i];
                                frm_bi_mv[block_y + j][block_x + i].bw_mv = intra_mv;
                            }
                            else if ( currMB->b8_ref[j][i] == BW )
                            {
                                frm_bi_mv[block_y + j][block_x + i].fw_mv = intra_mv;
                                frm_bi_mv[block_y + j][block_x + i].bw_mv = img->mv[BW][best_mode][j][i];
                            }
                            else
                            {
                                frm_bi_mv[block_y + j][block_x + i] = img->bi_mv[best_mode][j][i];
                            }
                        }
                    }
                }
#ifdef FastME
                if( input->UseFME == 1 )
                {
                    if( input->ABTen && input->InterABTen )
                    {
                        for ( j=0; j<4; j++ )
                        {
                            if( best_mode == INTER8x8 && currMB->subblock_type[j] == SUBBLOCK_INTER4x4 )
                            {
                                for ( i=0; i<4; i++ )
                                {
                                    if ( currMB->b4_ref[j][i] == FW )
                                    {
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = img->mv_4x4[FW][j][i];
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = intra_mv;
                                    }
                                    else if ( currMB->b4_ref[j][i] == BW )
                                    {
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = intra_mv;
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = img->mv_4x4[BW][j][i];
                                    }
                                    else
                                    {
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->bi_mv_4x4[j][i];
                                    }
                                }
                            }
                            else
                            {
                                for ( i=0; i<4; i++ )
                                {
                                    if ( currMB->b8_ref[j/2][j%2] == FW )
                                    {
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = img->mv[FW][best_mode][j/2][j%2];
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = intra_mv;
                                    }
                                    else if ( currMB->b8_ref[j/2][j%2] == BW )
                                    {
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = intra_mv;
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = img->mv[BW][best_mode][j/2][j%2];
                                    }
                                    else
                                    {
                                        curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->bi_mv[best_mode][j/2][j%2];
                                    }
                                }
                            }
                        }
                    }
                    else
                    {
                        for ( j=0; j<4; j++ )
                            for ( i=0; i<4; i++ )
                            {
                                if ( currMB->b8_ref[j/2][j%2] == FW )
                                {
                                    curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = img->mv[FW][best_mode][j/2][j%2];
                                    curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = intra_mv;
                                }
                                else if ( currMB->b8_ref[j/2][j%2] == BW )
                                {
                                    curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].fw_mv = intra_mv;
                                    curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2].bw_mv = img->mv[BW][best_mode][j/2][j%2];
                                }
                                else
                                {
                                    curr_frm_bi_mv_4x4[block_y + j/2][block_x + j%2][i/2][i%2] = img->bi_mv[best_mode][j/2][j%2];
                                }
                            }
                    }
                }
#endif
                break;
        }
    }
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/

static void decide_best_mode( int intra )
{
    static const int mb_mode_table[MAX_MODE_NUM] = { SKIP, INTER16x16, INTER8x8 };
    int index, mode;
    double min_rdcost    = MAX_VALUE;
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];
    double rdcost;
    if( !intra )
    {
        if ( input->rdopt )
        {
            for ( index = 0; index < 3; index++ )
            {
                mode = mb_mode_table[index];
                if ( valid[mode] )
                {
                    rdcost = rdcost_for_macroblocks( mode );
#if RDO_STAT
                    if( mode==0 )
                        printf( "MB:%3d,  Mode: SKIP,       Rdcost: %6.2f\n\n", img->curr_mb_nr, rdcost );
                    else if( mode==1 )
                        printf( "MB:%3d,  Mode: INTER16x16, Rdcost: %6.2f\n\n", img->curr_mb_nr, rdcost );
                    else if( mode==2 )
                        printf( "MB:%3d,  Mode: INTER8x8,   Rdcost: %6.2f\n\n", img->curr_mb_nr, rdcost );
#endif

                    if ( rdcost < min_rdcost )
                    {
                        min_rdcost = rdcost;
                        store_mb_para( mode );
                        set_best1_to_best0();
                    }
                }
            }
            if ( input->rc_enable && img->type == P_IMG )
            {
                get_residual( INTER16x16, img->org_y_data, inter_pred, diffy );
                img->mad_of_mb[img->curr_mb_nr] = calc_mad();
                if ( input->basic_unit < img->total_mb_num )
                {
                    img->total_mad_basic_unit += img->mad_of_mb[img->curr_mb_nr];
                    if ( currMB->cbp != 0 )
                    {
                        currMB->prev_cbp = 1;
                    }
                    else
                    {
                        img->qp         -= currMB->delta_qp;
                        currMB->delta_qp = 0;
                        currMB->qp       = img->qp;
                        currMB->prev_cbp = 0;
                    }
                }
            }
            set_stored_mb_para();
            set_best0_to_qadjust();
#if TRACE
            if ( 1 )
            {
                int i;
                if ( p_trace )
                {
                    fprintf( p_trace, "Best ref is: %i \n\n", currMB->b8_ref[0][0] );
                }
            }
#endif
        }
        else
        {
            set_mode_and_ref( best_mode, currMB );
            inter_y_residual_coding( NON_ABT );
            inter_uv_residual_coding();
            if ( input->rc_enable && img->type == P_IMG )
            {
                if ( currMB->cbp != 0 )
                    currMB->prev_cbp = 1;
                else
                {
                    img->qp         -= currMB->delta_qp;
                    currMB->delta_qp = 0;
                    currMB->qp       = img->qp;
                    currMB->prev_cbp = 0;
                }
            }
        }
        set_final_mv_ref( currMB->mb_type );
    }
    else //intra
    {
        if ( input->rdopt )
        {
            rdcost_for_macroblocks( I8MB );
            set_best1_to_qadjust();
        }
        else
        {
            intra_luma_residual_coding( NON_ABT );
            intra_chroma_residual_coding();
        }
        set_final_mv_ref( I8MB );
    }
#ifdef FastME
    if( input->UseFME == 1 )
    {
        skip_intrabk_SAD( currMB->mb_type, input->max_num_references );
    }
#endif
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
static void get_valid_mode( int intra )
{
    /* conditions for skipping mode (this is the same as MPEG-2)
    1.P frame: no motion vectors -> skip
    2.B frame:
    * 1) must be frame predicted
    * 2) must be the same prediction type (forward/backward/interp.)
    *   as previous macroblock
    * - relevant vectors (forward/backward/both) have to be the same
    *   as in previous macroblock
    */

    if ( !intra )
    {
        valid[SKIP]       = input->skip_mode_flag;
        valid[INTER16x16] = input->inter_search_16x16;
        valid[INTER8x8]   = input->inter_search_8x8;
        valid[I8MB]       = 1;
        valid[I4MB]       = ( input->ABTen && input->InterABTen ) ? 1 : 0;
    }
    else
    {
        memset( valid, 0, sizeof( valid ) );
    }
}

/*
*************************************************************************
* Function:
* Input:
* Output:
* Return:
* Attention:
*************************************************************************
*/
void encode_one_macroblock( int intra )
{
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];

    get_valid_mode( intra );

    decide_best_mode( intra );

    if ( !intra && IS_INTRA( currMB ) )
        intra_num[img->type-1]++;
}


void init_bs( bitstream_t *target_stream )
{
    byte *buffer = target_stream->stream_buffer;
    memset( target_stream, 0, sizeof( bitstream_t ) );
    target_stream->bits_to_go = 8;
    target_stream->stream_buffer = buffer;
}

